char foo;
#include <Arduino.h>

#include "Logging.h" 
#include <SoftwareSerial.h> //Software Serial Port

#include "TimerOne.h" 
#include "CurrentRGBCode.h" 
//#inlcude "RGBColor.h"

#define RxD 6 // This is the pin that the Bluetooth (BT_TX) will transmit to the Arduino (RxD)
#define TxD 7 // This is the pin that the Bluetooth (BT_RX) will receive from the Arduino (TxD)

SoftwareSerial blueToothSerial(RxD,TxD);

CurrentRGBCode currentRGBCode(0,0,0);

// automated Loop
bool automatedLoop = false;
int automatedLoopStep = 0;
RGBCode startRGBCode(0,0,0);
RGBCode targetRGBCode(0,0,0);

void setup() {
	Logging::setup();
	pinMode(RxD, INPUT); 
	pinMode(TxD, OUTPUT); 
	setupBlueToothConnection();
	// led check
	currentRGBCode.setRGBValue(255,255,255);
	delay(500);
	currentRGBCode.setRGBValue(0,0,0);
}

void loop() {
	while(1) {
		// data available on Socket?
		if(blueToothSerial.available()) {
			char recvChar = blueToothSerial.read();
			// check if this is the beginning of a message
			if(recvChar == 0x02) {
				receiveMessage();
			}
		}
		delay(25);
		if(automatedLoop) {
			automatedLED();
		}
	}
}

/**
	setup Bluetoothshield
*/
void setupBlueToothConnection() {
	/**
		Setup configuration:
		http://blog.iteadstudio.com/upload-program-to-arduino-with-bt-shield/
	*/

	blueToothSerial.begin(9600); 
	// restore default state
	blueToothSerial.print("AT+ORGL"); 
	// work as slave
	blueToothSerial.print("AT+ROLE=0"); 
	blueToothSerial.print("AT+POLAR=1,0"); 
	blueToothSerial.print("AT+INIT");
	blueToothSerial.print("AT+UART=9600,0,0"); 
	Logging::writeInfoLog("Bluetoothshield setup.");
	blueToothSerial.flush();
}

/**
	receive Message
*/
void receiveMessage() {
	char recvChar;
	bool whileEnd = false;
	
	int maxMessageLen = 20;
	char message[maxMessageLen + 1];
	int messageLen = 0;
	
	// read socket until not data is available
	delay(100);
	while(blueToothSerial.available() && !whileEnd) {
		// fast read
		while(blueToothSerial.available() && !whileEnd) {
			recvChar = blueToothSerial.read();
			
			// log
			logMessage = "Received char: '";
			logMessage += recvChar;
			logMessage += "' - value: ";
			logMessage += (int)recvChar;
			Logging::writeDebugLog(logMessage);

			// ETX received? end of message
			if(recvChar == 0x03) {
				whileEnd = true;
				break;
			}

			if(messageLen <= maxMessageLen) {
				message[messageLen] = recvChar;
				messageLen++;
			} else {
				Logging::writeErrorLog("Max message buffer reached!");
				whileEnd = true;
				break;
			}
		}
		delay(50);
	}

	message[messageLen] = '\0';

	logMessage = "Received Message: '";
	logMessage += message;
	logMessage += "'";
	Logging::writeInfoLog(logMessage);
	
	// process message
	processMessage(message, messageLen);
}

void processMessage(char *message, int messageLen) {
	switch (message[0]) {
		// Red LED
		case 'R':
			Logging::writeInfoLog("R");
			processSingleLed(message, messageLen, 'R');
		break;
		// Green LED
		case 'G':
			Logging::writeInfoLog("G");
			processSingleLed(message, messageLen, 'G');
		break;
		// Blue LED
		case 'B':
			Logging::writeInfoLog("B");
			processSingleLed(message, messageLen, 'B');
		break;
		// RGB LED
		case 'L':
			Logging::writeInfoLog("L");
			processRGBLed(message, messageLen);
		break;
		// Return current color value
		case 'C':
			Logging::writeInfoLog("C");
			processRGBLed(message, messageLen);
		break;
		// automated color gradient
		case 'A':
			Logging::writeInfoLog("A");
			processAutomatedLED(message, messageLen);
		break;
		// unknown command?
		default:
			logMessage = "Unknown command: '";
			logMessage += message[0];
			logMessage += "' full message: '";
			logMessage += message;
			logMessage += "'";
			Logging::writeErrorLog(logMessage);
		break;
	}
}

/**
	processing single LED command
*/
void processSingleLed(char *message, int messageLen, char color) {
	if(messageLen >= 4) {
		switch (color) {
			case 'R':
				currentRGBCode.setRedValueChar(message + 1);
			break;
			case 'G':
				currentRGBCode.setGreenValueChar(message + 1);
			break;
			case 'B':
				currentRGBCode.setBlueValueChar(message + 1);
			break;
		}
	}
}

/**
	processing RGB Led command
	telegram from device:
	L255070200 -> L 255(R) 070(G) 200(B)
*/
void processRGBLed(char *message, int messageLen) {
	if(messageLen >= 10) {
		currentRGBCode.setRGBValueChar(message + 1);
	}
}

/**
	set Value on LedPin
*/
void processAutomatedLED(char *message, int messageLen) {
	if(messageLen >= 2) {
		Serial.println("Message");
		switch(message[1]) {
			// start automated LED
			case 'S':
				Logging::writeInfoLog("Started automated LED");
				automatedLoop = true;
				automatedLoopStep = 0;
			break;
			// stop automated LED
			case 'E':
				stopAutomatedLED();
			break;
		}
	}
}

void stopAutomatedLED() {
	Logging::writeInfoLog("Stopped automated LED");
	automatedLoop = false;
	Timer1.detachInterrupt();
}

void automatedLED() {
	//while(1) {
	if((currentRGBCode.getRedValue() == targetRGBCode.getRedValue() 
	&& currentRGBCode.getGreenValue() == targetRGBCode.getGreenValue() 
	&& currentRGBCode.getBlueValue() == targetRGBCode.getBlueValue())
	|| automatedLoopStep == 0) {
		switch(automatedLoopStep) {
			// start value
			case 0:
				targetRGBCode.setRGBValue(255,0,0);
				automatedLoopStep++;
			break;
			case 1:
				Logging::writeDebugLog("Automated Loop: Next - Red");
				targetRGBCode.setRGBValue(255,0,0);
			break;
			case 2:
				Logging::writeDebugLog("Automated Loop: Next - Red + Green");
				targetRGBCode.setRGBValue(255,255,0);
			break;
			case 3:
				Logging::writeDebugLog("Automated Loop: Next - Green");
				targetRGBCode.setRGBValue(0,255,0);
			break;
			case 4:
				Logging::writeDebugLog("Automated Loop: Next - Green + Blue");
				targetRGBCode.setRGBValue(0,255,255);
			break;
			case 5:
				Logging::writeDebugLog("Automated Loop: Next - Blue");
				targetRGBCode.setRGBValue(0,0,255);
			break;
			case 6:
				Logging::writeDebugLog("Automated Loop: Next - Blue + Red");
				targetRGBCode.setRGBValue(255,0,255);
				automatedLoopStep = 0;
			break;
		}
		startRGBCode.setRGBValue(currentRGBCode.getRedValue(), currentRGBCode.getGreenValue(), currentRGBCode.getBlueValue());
		automatedLoopStep++;	
	}
	
	// calculate step size
	currentRGBCode.setRGBValue(
		calculateNextStepValue(currentRGBCode.getRedValue(), startRGBCode.getRedValue(), targetRGBCode.getRedValue()),
		calculateNextStepValue(currentRGBCode.getGreenValue(), startRGBCode.getGreenValue(), targetRGBCode.getGreenValue()),
		calculateNextStepValue(currentRGBCode.getBlueValue(), startRGBCode.getBlueValue(), targetRGBCode.getBlueValue()));
	//delay(100);
	//}
}

int calculateNextStepValue(int currentValue, int startValue, int targetValue) {
	// 50 Steps (= 5 seconds)
	int nextValue = currentValue + ((targetValue - startValue) / 50);
	if(nextValue < 0) {
		nextValue = 0;
	} 
	if(nextValue > 255) {
		nextValue = 255;
	}
	return nextValue;
}

// old Bluetooth Setup
/*
void setupBlueToothConnection() {
	blueToothSerial.begin(38400); //Set BluetoothBee BaudRate to default baud rate 38400
	blueToothSerial.print("\r\n+STWMOD=0\r\n"); //set the bluetooth work in slave mode
	blueToothSerial.print("\r\n+STNA=SeeedBTSlave\r\n"); //set the bluetooth name as "SeeedBTSlave"
	blueToothSerial.print("\r\n+STOAUT=1\r\n"); // Permit Paired device to connect me
	blueToothSerial.print("\r\n+STAUTO=0\r\n"); // Auto-connection should be forbidden here
	delay(2000); // This delay is required.
	blueToothSerial.print("\r\n+INQ=1\r\n"); //make the slave bluetooth inquirable 
	Serial.println("The slave bluetooth is inquirable!");
	delay(2000); // This delay is required.
	blueToothSerial.flush();
}
*/
